# novapi
#  |- timer
#     |- timer()
#     |- reset_timer()
#  |- gyro_sensor
#     |- get_pitch()
#     |- get_roll()
#     |- get_yaw()
#     |- is_shaked()
#     |- get_acceleration(axis)
#     |- get_gyroscope(axis)
#     |- set_shake)threshold(threshold)
#     |- reset_rotation(axis)
# mbuild
#  |- 1
# 在主程序中调用其他模块，并将主程序中的代码部分复制进慧编程即可

"""
import serial
import serial.tools.list_ports
"""

# 以下代码复制进慧编程（可做修改）
from mbuild import gamepad
from mbuild.encoder_motor import encoder_motor_class

# 初始化变量
i = 0


def judgement():
    global i
    if gamepad.is_key_pressed("L1"):
        i = 1
    if gamepad.is_key_pressed("L2"):
        i = 0


def pid(var):
    a = (9.8 * (var ** 2)) / (2 * (2.21 ** 2))
    return a


encoder_motor_M1 = encoder_motor_class("M1", "INDEX1")
encoder_motor_M2 = encoder_motor_class("M2", "INDEX1")
encoder_motor_M3 = encoder_motor_class("M3", "INDEX1")
encoder_motor_M4 = encoder_motor_class("M4", "INDEX1")
encoder_motor_M5 = encoder_motor_class("M5", "INDEX1")
encoder_motor_M6 = encoder_motor_class("M6", "INDEX1")

# 判断是否为主程序
if __name__ == "__main__":
    print("-----|程序开始运行|-----")
    while True:
        time.sleep(0.001)
        encoder_motor_M1.set_power(-1 * pid())
        encoder_motor_M2.set_power((((-1 * (
                    (9.8 * (gamepad.get_joystick("Lx") * gamepad.get_joystick("Lx"))) / (2 * (2.21 * 2.21))) + -1 * (
                                                  (9.8 * (gamepad.get_joystick("Ly") * gamepad.get_joystick("Ly"))) / (
                                                      2 * (2.21 * 2.21))))) + 0 * (
                                                (9.8 * (gamepad.get_joystick("Rx") * gamepad.get_joystick("Rx"))) / (
                                                    2 * (2.21 * 2.21)))))
        encoder_motor_M3.set_power((((-1 * (
                    (9.8 * (gamepad.get_joystick("Lx") * gamepad.get_joystick("Lx"))) / (2 * (2.21 * 2.21))) + 0 * (
                                                  (9.8 * (gamepad.get_joystick("Ly") * gamepad.get_joystick("Ly"))) / (
                                                      2 * (2.21 * 2.21))))) + 1 * (
                                                (9.8 * (gamepad.get_joystick("Rx") * gamepad.get_joystick("Rx"))) / (
                                                    2 * (2.21 * 2.21)))))
        encoder_motor_M4.set_power((((-1 * (
                    (9.8 * (gamepad.get_joystick("Lx") * gamepad.get_joystick("Lx"))) / (2 * (2.21 * 2.21))) + -1 * (
                                                  (9.8 * (gamepad.get_joystick("Ly") * gamepad.get_joystick("Ly"))) / (
                                                      2 * (2.21 * 2.21))))) + 0 * (
                                                (9.8 * (gamepad.get_joystick("Rx") * gamepad.get_joystick("Rx"))) / (
                                                    2 * (2.21 * 2.21)))))
        judgement()
        if i == 1:
            encoder_motor_M5.set_power(-50)
            encoder_motor_M6.set_power(50)
        else:
            encoder_motor_M5.set_power(0)
            encoder_motor_M6.set_power(0)
